package com.google.android.apps.skipbot;

/**
 * Encapsulates the state of a servo and the ability to manipulate it.
 */
public class Servo {
  /** The interface to control the servo. **/
  private final RobotController robotController;
  /** The logical servo number. **/
  private final int index;
  /** The pin the servo is attached to. **/
  private final int pin;
  /** The shift from desired angle to the send angle for the servo. **/
  private final int calibrationShift;
  /** If the direction of the servo should be flipped. **/
  private final boolean flipped;
  /** Indicates if the servo is currently attached. **/
  private boolean attached = false;
  /**
   * The current angle of the servo in degrees. This number is irrelevant when
   * detached.
   **/
  private int currentAngle = 0;

  private static final int MIN_SEND_ANGLE = 0;
  private static final int MAX_SEND_ANGLE = 180;

  public Servo(RobotController robotController, int index, int pin, int calibrationShift,
      boolean flipped) {
    this.robotController = robotController;
    this.index = index;
    this.pin = pin;
    this.calibrationShift = calibrationShift;
    this.flipped = flipped;
  }

  /**
   * Moves the given servo to the given angle.
   * 
   * @param angle angle to move the servo
   */
  public void moveServo(int angle) {
    robotController.moveServo(index, getSendAngle(angle));
    currentAngle = angle;
  }

  /**
   * Attaches a the servo to the pin specified in the robot config.
   */
  public void attach() {
    robotController.attachServoToPin(index, pin);
    attached = true;
  }

  /**
   * Detaches a the servo.
   */
  public void detach() {
    robotController.detachServo(index);
    attached = false;
  }

  /**
   * Gets the index of the servo.
   */
  public int getIndex() {
    return index;
  }

  /**
   * Gets the pin the servo is attached to.
   */
  public int getPin() {
    return pin;
  }

  /**
   * Gets if the servo is attached.
   */
  public boolean isAttached() {
    return attached;
  }

  /**
   * Gets the current angle of the servo. This may be different than the actual
   * angle of the servo, but it is the angle that we are telling the servo to be
   * in.
   */
  public int getCurrentAngle() {
    return currentAngle;
  }

  /**
   * Sets the internal value for the current angle. This does not actually move
   * the servo to this position and should only be used to sync the state of the
   * servo object if the actual servo was moved by some external command such as
   * moveAllServos.
   * 
   * @param currentAngle the angle to set the servo's internal state to
   */
  public void setCurrentAngle(int currentAngle) {
    this.currentAngle = currentAngle;
  }

  /**
   * Gets the calibration shift of the servo that shifts a desired angle to the
   * actual angle that needs to be sent to the robot servo. Desired Angle +
   * Calibration Shift = Send Angle.
   */
  public int getCalibrationShift() {
    return calibrationShift;
  }

  /**
   * Gets if the direction of the servo is flipped.
   */
  public boolean getFlipped() {
    return flipped;
  }

  /**
   * Utility method to create an array of servo objects based on a robot config.
   * 
   * @param robotController object that gives controls to manipulate a robot
   * @param robotConfig configuration of a robot
   * @return an array of Servo objects
   */
  public static Servo[] createServoArray(RobotController robotController, RobotConfig robotConfig) {
    int numServos = robotConfig.getNumServos();
    Servo[] servos = new Servo[numServos];
    for (int i = 0; i < numServos; ++i) {
      servos[i] =
          new Servo(robotController, i, robotConfig.getPin(i), robotConfig.getCalibrationShift(i),
              robotConfig.getDirectionFlipped(i));
    }
    return servos;
  }

  /**
   * Gets the angle that should be sent to the robot controller for a desired
   * angle taking calibration into account.
   * 
   * @param desiredAngle the angle that is desired
   * @return the angle that should be sent to the robot to get the desired angle
   */
  public int getSendAngle(int desiredAngle) {
    int sendAngle = desiredAngle + (flipped ? -calibrationShift : calibrationShift);
    sendAngle = flipped ? 180 - sendAngle : sendAngle;
    sendAngle = sendAngle < MIN_SEND_ANGLE ? MIN_SEND_ANGLE : sendAngle;
    sendAngle = sendAngle > MAX_SEND_ANGLE ? MAX_SEND_ANGLE : sendAngle;
    return sendAngle;
  }
}
